#include "myInit.h"

/* FDCAN中断 ***************************************************************/

void HAL_FDCAN_RxFifo0Callback(FDCAN_HandleTypeDef *hfdcan, uint32_t RxFifo0ITs)
{
    if ((hfdcan->Instance == FDCAN1) || (hfdcan->Instance == FDCAN2) || (hfdcan->Instance == FDCAN3))
    {
		static user_FDCAN_Rx_Struct FDCAN_Rx_struct = {0};          //创建CAN接收结构体
		static uint8_t ID = 0;
	
		user_get_FDCAN_FIFO0_Msgs(hfdcan, &FDCAN_Rx_struct);    //接收CAN报文
		
		if (!gMotorRxInitFinishFlag)    //如果电机接收初始化没完成, 不处理接收到的数据
		{
			return;
		}
        ID = 0;
        ID += VESC_Analysis(&FDCAN_Rx_struct);
		ID += DJI_Analysis(&FDCAN_Rx_struct);
		
		switch (ID)
		{
			case ClawLID:
				ClawLRxFlag = 1;
				break;
			
			case ClawRID:
				ClawRRxFlag = 1;
				break;
			
			case EscaladeLID:
				EscaladeLFlag = 1;
				break;
			
			case EscaladeRID:
				EscaladeRFlag = 1;
				break;
			
			default:
				break;
		}
        if (ID != 0)
        {
            for (uint8_t i = 0; i < CANNum; i++)
            {
                if (ID == IDArray[i])
                {
                    IDFlagArray[i] = 1;
                    return;
                }
            }
        }
    }
}

/* 定时器中断 ***************************************************************/

void my_HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
    static uint16_t sCount500 = 0;

    static float sShootNowAngle_buff = 0.f;
    static float sShootDetlaAngle = 0.f;
    static float sShootSpeedLastAngle = 0.f;

    if(htim == &htim5)
    {
		{	//当检测到FDCAN总线因发送错误过多自动关闭时重启
			if (hfdcan1.Instance->PSR & FDCAN_PSR_BO)		
			{
				HAL_FDCAN_Stop(&hfdcan1);
				USER_FDCAN1_INIT(&hfdcan1);
			}
			if (hfdcan2.Instance->PSR & FDCAN_PSR_BO)
			{
				HAL_FDCAN_Stop(&hfdcan2);
				USER_FDCAN2_INIT(&hfdcan2);
			}
			if (hfdcan3.Instance->PSR & FDCAN_PSR_BO)
			{
				HAL_FDCAN_Stop(&hfdcan3);
				USER_FDCAN3_INIT(&hfdcan3);
			}
		}
		

        /* 500ms检查一次电机是否连接 */
        sCount500++;
        if (sCount500 >= 250)
        {
            sCount500 = 0;
			gErrState.shootErr = false;
            for (uint8_t i = 0; i < CANNum; i++)
            {
                if (IDFlagArray[i] == 0)        //电机通讯错误
                {
                    switch (IDArray[i])
                    {
                        case ShootID:
							gErrState.shootErr = true;
                            //推射电机通讯错误, 接收不到推射电机传回来的数据
                            break;
						
						default:
							break;

                    }
                    //MY_ERROR(); 错误处理的地方

                } 
                IDFlagArray[i] = 0;
            }
        }
        /*****************************/

        /* 推射编码器读角度 */

		if (gMechInitFinishAndStartEncFlag)		//如果推射电机设置完原点
		{
			if (shootEnc.pf->read(&shootEnc))               //从推射VESC获得转发的角度
			{
				gShootReadAngle = shootEnc.angle; 

				sShootDetlaAngle = gShootReadAngle - gShootLastAngle;
				if (sShootDetlaAngle > 180)
				{
					sShootNowAngle_buff -= 360;
				}
				else if (sShootDetlaAngle < -180)
				{
					sShootNowAngle_buff += 360;
				}

				sShootNowAngle_buff += sShootDetlaAngle;

				gShootNowAngle = sShootNowAngle_buff;
				gShootLastAngle = gShootReadAngle;
			}
			else
			{
				
				static float ssShootFalseCount10 = 0;
				ssShootFalseCount10++;
				if (ssShootFalseCount10 >= 10)
				{
					gErrState.shootErr = true;
					//推射通讯错误 接收不到数据十次 20ms
				}
			}

			#if ENC_SPEED_ENABLE    //用编码器测推射电机的转速
				static uint8_t sCount100 = 0;
				sCount100++;
				if (sCount100 == 50)
				{
					sCount100 = 0;
					gShootSpeed = (gShootNowAngle - sShootSpeedLastAngle) * 10.f / 6.f;
					sShootSpeedLastAngle = gShootNowAngle;
				}
			#endif
			/********************/
		
		}
    }
}


/* FDCAN发送任务 ***************************************************************/
#define refreshCAN1()	{											\
	while(xQueueReceive(other_CAN1Handle,&sMotorToCAN,1) == pdTRUE)	\
	{																\
		sRxDataFlag = 1;											\
		switch(sMotorToCAN.ID)										\
		{															\
			case WheelFID:											\
				sWheelData[0].Data = sMotorToCAN.Data;				\
				sWheelData[0].eMode = sMotorToCAN.eMode;			\
				break;												\
			case WheelRID:											\
				sWheelData[1].Data = sMotorToCAN.Data;				\
				sWheelData[1].eMode = sMotorToCAN.eMode;			\
				break;												\
			case WheelBID:											\
				sWheelData[2].Data = sMotorToCAN.Data;				\
				sWheelData[2].eMode = sMotorToCAN.eMode;			\
				break;												\
			case WheelLID:											\
				sWheelData[3].Data = sMotorToCAN.Data;				\
				sWheelData[3].eMode = sMotorToCAN.eMode;			\
				break;												\
																	\
			case HelmFID:											\
				sHelmData[0].Data = sMotorToCAN.Data;				\
				break;												\
			case HelmRID:											\
				sHelmData[1].Data = sMotorToCAN.Data;				\
				break;												\
			case HelmBID:											\
				sHelmData[2].Data = sMotorToCAN.Data;				\
				break;												\
			case HelmLID:											\
				sHelmData[3].Data = sMotorToCAN.Data;				\
				break;												\
																	\
			default:												\
				break;												\
		}															\
	}																\
	if(sRxDataFlag) 												\
	{																\
		sRxDataFlag = 0;											\
		osDelay(1);													\
	}																\
}						

extern float gClawAimAngle; 
extern uint32_t gClawDeathWaitTime;

void can1Task(void *argument)
{
	static MotorToCAN sMotorToCAN;
	static MotorToCAN sWheelData[4] = {{.eMode = eMotorMode_Speed}, {.eMode = eMotorMode_Speed}, {.eMode = eMotorMode_Speed}, {.eMode = eMotorMode_Speed}};
	static MotorToCAN sHelmData[4];
	static uint8_t sRxDataFlag = 0;
	
    for(;;)
    {
//		if ((gClawAimAngle == cClawHalfCloseAngle) && (xTaskGetTickCount() > gClawDeathWaitTime + 300))
//		{
//			closeHalfClawLowCurrent(osDelay, 1);
//			gClawAimAngle = 0;
//		}
//		if ((gClawAimAngle == cClawCloseAngle) && (xTaskGetTickCount() > gClawDeathWaitTime + 300))
//		{
//			closeClawLowCurrent(osDelay, 1);
//			gClawAimAngle = 0;
//		}
		
        while (FDCAN1->PSR & FDCAN_PSR_BO) 
        {
            HAL_FDCAN_Stop(&hfdcan1);
            HAL_FDCAN_DeInit(&hfdcan1);
            __HAL_RCC_FDCAN_CLK_DISABLE();
            __HAL_RCC_FDCAN_FORCE_RESET();
            __HAL_RCC_FDCAN_RELEASE_RESET();
            __HAL_RCC_FDCAN_CLK_ENABLE();
            osDelay(10);
            HAL_FDCAN_Init(&hfdcan1);
            HAL_FDCAN_ConfigGlobalFilter(&hfdcan1, FDCAN_ACCEPT_IN_RX_FIFO0, FDCAN_ACCEPT_IN_RX_FIFO0, FDCAN_FILTER_REMOTE, FDCAN_FILTER_REMOTE);
            HAL_FDCAN_ActivateNotification(&hfdcan1, FDCAN_IT_RX_FIFO0_NEW_MESSAGE, 0);
            HAL_FDCAN_Start(&hfdcan1);
            osDelay(10);
        }
		
		refreshCAN1();
		if (sWheelData[0].eMode == eMotorMode_Speed)
			WheelF.pf->speedMode(&WheelF, sWheelData[0].Data);
		else if (sWheelData[0].eMode == eMotorMode_Pos)
			WheelF.pf->posMode(&WheelF, sWheelData[0].Data);
		else if (sWheelData[0].eMode == eMotorMode_Duty)
			WheelF.pf->dutyMode(&WheelF, sWheelData[0].Data);
		else if (sWheelData[0].eMode == eMotorMode_Block)
			WheelF.pf->staticBreakMode(&WheelF, 15);
		else if (sWheelData[0].eMode == eMotorMode_Break)
			WheelF.pf->dynamicBreakMode(&WheelF, 30);
		
		refreshCAN1();
		if (sWheelData[1].eMode == eMotorMode_Speed)
			WheelR.pf->speedMode(&WheelR, sWheelData[1].Data);
		else if (sWheelData[1].eMode == eMotorMode_Pos)
			WheelR.pf->posMode(&WheelR, sWheelData[1].Data);
		else if (sWheelData[1].eMode == eMotorMode_Duty)
			WheelR.pf->dutyMode(&WheelR, sWheelData[1].Data);
		else if (sWheelData[1].eMode == eMotorMode_Block)
			WheelR.pf->staticBreakMode(&WheelR, 15);
		else if (sWheelData[1].eMode == eMotorMode_Break)
			WheelR.pf->dynamicBreakMode(&WheelR, 30);
		
		refreshCAN1();
		if (sWheelData[2].eMode == eMotorMode_Speed)
			WheelB.pf->speedMode(&WheelB, sWheelData[2].Data);
		else if (sWheelData[2].eMode == eMotorMode_Pos) 
			WheelB.pf->posMode(&WheelB, sWheelData[2].Data);
		else if (sWheelData[2].eMode == eMotorMode_Duty)
			WheelB.pf->dutyMode(&WheelB, sWheelData[2].Data);
		else if (sWheelData[2].eMode == eMotorMode_Block)
			WheelB.pf->staticBreakMode(&WheelB, 15);
		else if (sWheelData[2].eMode == eMotorMode_Break)
			WheelB.pf->dynamicBreakMode(&WheelB, 30);
		
		refreshCAN1();
		if (sWheelData[3].eMode == eMotorMode_Speed)
			WheelL.pf->speedMode(&WheelL, sWheelData[3].Data);
		else if (sWheelData[3].eMode == eMotorMode_Pos) 
			WheelL.pf->posMode(&WheelL, sWheelData[3].Data);
		else if (sWheelData[3].eMode == eMotorMode_Duty)
			WheelL.pf->dutyMode(&WheelL, sWheelData[3].Data);
		else if (sWheelData[3].eMode == eMotorMode_Block)
			WheelL.pf->staticBreakMode(&WheelL, 15);
		else if (sWheelData[3].eMode == eMotorMode_Break)
			WheelL.pf->dynamicBreakMode(&WheelL, 30);
		
#define cHelmSpeedLimit 		120.f				//速度限制
		refreshCAN1();
		HelmF.pf->posMode(&HelmF, sHelmData[0].Data, 0, 0);
		refreshCAN1();                             
		HelmR.pf->posMode(&HelmR, sHelmData[1].Data, 0, 0);
		refreshCAN1();                             
		HelmB.pf->posMode(&HelmB, sHelmData[2].Data, 0, 0);
		refreshCAN1();                             
		HelmL.pf->posMode(&HelmL, sHelmData[3].Data, 0, 0);
    }
}


/* LED函数 ***************************************************************/
uint8_t Aim_LED;
#define LED_COLOR_WHITE 	0xff, 0xff, 0xff
#define LED_COLOR_BLUE 		0x55, 0x00, 0x00
#define LED_COLOR_RED 		0x00, 0x55, 0x00
#define LED_COLOR_GREEN 	0x00, 0x00, 0x55
#define LED_COLOR_PINK 	    0x44, 0xff, 0x11
#define LED_COLOR_YELLOW 	0x00, 0x77, 0x44
void ledTask(void *argument)
{
    static uint16_t sTick = 0;
    while (1)
    {
    #if SHOOT_ENABLE
        if (xTaskGetTickCount() < sTick + 500)
        {
            if (gErrState.twoCarErr)
            {
                LEDStrip.pf->setColor(&LEDStrip, 1, 13, LED_COLOR_PINK);    	//两车通信错误, 红灯
            }
        } 
        else if ((xTaskGetTickCount() >= (sTick + 500)) && (xTaskGetTickCount() < (sTick + 1000)))
        {
            if (gErrState.shootErr)
            {
                LEDStrip.pf->setColor(&LEDStrip, 1, 13, LED_COLOR_YELLOW);    	//推射错误, 黄灯
            }
        }  
        else if ((xTaskGetTickCount() >= (sTick + 1000)) && (xTaskGetTickCount() < (sTick + 1500)))
        {
            if (gErrState.pcErr)
            {
                LEDStrip.pf->setColor(&LEDStrip, 1, 13, LED_COLOR_RED);    		//小电脑错误, 红灯
            }
        } 
		
    #endif
        else 
        {
            sTick = xTaskGetTickCount();
        }

        if (!gErrState.pcErr && !gErrState.shootErr && !gErrState.twoCarErr)
        {
            LEDStrip.pf->setColor(&LEDStrip, 1, 13, LED_COLOR_BLUE);            // 没有错误, 绿灯    
        }
		
        switch(Aim_LED)
        {
            case 0x0A:  // 蓝色
                        LEDStrip.pf->setColor(&LEDStrip, 14, 20, LED_COLOR_BLUE);
                        break;
            case 0x0B:  // 白色
                        LEDStrip.pf->setColor(&LEDStrip, 14, 20, LED_COLOR_WHITE);
                        break;
            case 0x0C:  // 绿色
                        LEDStrip.pf->setColor(&LEDStrip, 14, 20, LED_COLOR_GREEN);
                        break;
            case 0x0D:  // 红色
                        LEDStrip.pf->setColor(&LEDStrip, 14, 20, LED_COLOR_RED);
                        break;
            case 0x0E:  // 粉色
                        LEDStrip.pf->setColor(&LEDStrip, 14, 20, LED_COLOR_PINK);
                        break;
			
            default:    break;
        }
        osDelay(10);
    } 
}
